import math
import struct
import numpy as np
class roadPlanner():
    def __init__(self):
        self.a = 41.04  # a,b是轴2原点先对于轴1原点的位移
        self.b = 84.72
        self.l1 = 200
        self.l2 = 214.8
        self.motorStep = 1.8 / 16
        self.gearRate1 = 45 / 21
        self.gearRate2 = 45 / 11
        self.gearRate3 = 45 / 11
        self.robotPosition = [0, 0, 0]  # 记录机器人当前的坐标     这里需要修改初始化的值
        self.robotAngle = [0, 0, 0]  # 记录机器人当前电机的角度

    def solve(self,thetaList):
        theta1 = thetaList[0] * math.pi / 180
        theta2 = thetaList[1] * math.pi / 180
        theta3 = thetaList[2] * math.pi / 180
        x = (self.l1 * math.cos(theta2) - self.l2 * math.cos(theta3) + self.a) * math.cos(theta1)
        y = (self.l1 * math.cos(theta2) - self.l2 * math.cos(theta3) + self.a) * math.sin(theta1)
        z = self.l1 * math.sin(theta2) - self.l2 * math.sin(theta3) + self.b
        return x, y, z
    def inverseSolve(self,p):  # p需要反解的坐标
        x = p[0]
        y = p[1]
        z = p[2]
        # print(x,y,z)
        v1 = math.sqrt(x ** 2 + y ** 2) - self.a  # v1,v2为反解中的中间变量
        v2 = z - self.b
        # print(v1,v2)
        if x!=0:
            theta1 = math.atan(y / x)
        else:
            theta1 = math.pi/2
        theta2 = math.acos((v1 ** 2 + v2 ** 2 - self.l1 ** 2 - self.l2 ** 2) / (-2 * self.l1 * self.l2))
        theta3 = math.atan(v2 / v1) + math.acos((self.l1 ** 2 + v1 ** 2 + v2 ** 2 - self.l2 ** 2) / (2 * self.l1 * math.sqrt(v1 ** 2 + v2 ** 2)))
        print(theta1* 180 / math.pi,theta2 * 180 / math.pi,theta3 * 180 / math.pi)
        theta1 = theta1 * 180 / math.pi * self.gearRate1  # 由弧度制转化为角度值,并乘以齿轮传输比
        theta2 = theta2 * 180 / math.pi * self.gearRate2
        theta3 = theta3 * 180 / math.pi * self.gearRate3
        theta2 = theta3 + theta2
        # pulse1 = round(theta1 / self.motorStep)  # 将角度转化为1.8/16的整数倍
        # pulse2 = round(theta2 / self.motorStep)
        # pulse3 = round(theta3 / self.motorStep)
        # # print(pulse3)
        # theta1 = round(pulse1 * self.motorStep, 1)
        # theta2 = round(pulse2 * self.motorStep, 1)
        # theta3 = round(pulse3 * self.motorStep, 1)
        # print(theta3)
        return theta1, theta2, theta3

    def rototeFromTo(self,p1, p2, t):  # 从一点到另一点，计算每个轴需要转动的角度
        [alpha1, alpha2, alpha3] = self.inverseSolve(p1)
        [beta1, beta2, beta3] = self.inverseSolve(p2)
        theta1 = beta1 - alpha1
        theta2 = beta2 - alpha2
        theta3 = beta3 - alpha3
        # theta3=theta3+theta2
        v1 = theta1 / t  # 打包数据发出
        v2 = theta2 / t
        v3 = theta3 / t

        return math.fabs(theta1), math.fabs(theta2), math.fabs(theta3), v1, -v2, -v3

    def linePlanning(self,p1, p2, k, t):  # p1至p2沿直线，分成k个小位移
        px = np.linspace(p1, p2, k)
        print(px)
        thetaRoad = []

        for i in range(0, k - 1):
            theta = self.rototeFromTo(list(px[i]), list(px[i + 1]), t / k)
            thetaRoad.append(theta)
        return thetaRoad

    def circlePlanning(self,p, r, t, k):  # pi圆心，r半径
        x = p[0]
        y = p[1]
        z = p[2]
        angle = 360 / k / 180 * math.pi
        circlePointList = []
        for k in range(0, k):
            zx = 0.16
            pk = [0, 0, 0]
            pk[0] = x + r * math.cos(k * angle)
            pk[1] =( y + r * math.sin(k * angle))*0.85
            pk[2] = z - 0.16*(275 - x)
            print(pk)
            # inverseSolve(pk)
            circlePointList.append(pk)
        #print(circlePointList)
        thetaRoad = []
        for i in range(0, k - 1):
            theta = self.rototeFromTo(list(circlePointList[i]), list(circlePointList[i + 1]), t / k)
            thetaRoad.append(theta)
        #print(thetaRoad)
        return thetaRoad

    def pTop(self,p1, p2, t, k):
        # p1[2] = p1[2] - 0.16 * (275 - p1[0])
        # p2[2] = p2[2] - 0.16 * (275 - p2[0])
        theta1 = self.inverseSolve(p1)
        theta2 = self.inverseSolve(p2)
        print(theta1,theta2)
        theta = [0, 0, 0]
        theta[0] = theta2[0] - theta1[0]
        theta[1] = theta2[1] - theta1[1]
        theta[2] = theta2[2] - theta1[2]
        print(theta)
        thetaList = []
        vList = []
        for i in range(1, k+1):
            thetak = [0, 0, 0]
            vk = [0, 0, 0]
            thetak[0] = (3 / t ** 2) * theta[0] * ((2 * i - 1) / k ** 2 * t ** 2) - (2 / t ** 3) * theta[0] * (
                        (3 * i ** 2 - 3 * i + 1) / k ** 3 * t ** 3)
            thetak[1] = (3 / t ** 2) * theta[1] * ((2 * i - 1) / k ** 2 * t ** 2) - (2 / t ** 3) * theta[1] * (
                        (3 * i ** 2 - 3 * i + 1) / k ** 3 * t ** 3)
            thetak[2] = (3 / t ** 2) * theta[2] * ((2 * i - 1) / k ** 2 * t ** 2) - (2 / t ** 3) * theta[2] * (
                        (3 * i ** 2 - 3 * i + 1) / k ** 3 * t ** 3)
            thetak[0] = math.fabs(thetak[0])
            thetak[1] = math.fabs(thetak[1])
            thetak[2] = math.fabs(thetak[2])
            vk[0] = (6 / t ** 2) * theta[0] * (i / k * t) - (6 / t ** 3) * theta[0] * (i / k * t) ** 2
            vk[1] = (6 / t ** 2) * theta[1] * (i / k * t) - (6 / t ** 3) * theta[1] * (i / k * t) ** 2
            vk[2] = (6 / t ** 2) * theta[2] * (i / k * t) - (6 / t ** 3) * theta[2] * (i / k * t) ** 2
            vk[1] = -vk[1]
            vk[2] = -vk[2]
            thetaList.append(thetak)
            vList.append(vk)
        vList[k-1][0] = vList[k-2][0]
        vList[k-1][1] = vList[k-2][1]
        vList[k-1][2] = vList[k-2][2]
        return thetaList, vList
if __name__ =="__main__":
    #print(math.cos(90*math.pi/180))
    robotRoad=roadPlanner()
    # print(robotRoad.pTop([93,0,216],[200,50,100],20,5))
    # #print(robotRoad.inverseSolve([93,0,216]))
    # #print(robotRoad.inverseSolve([315,0,40]))
    # print(robotRoad.solve([0.0,83.93026480727636,41.03594783023134])
    robotRoad.circlePlanning([200,0,150],30,16,16)